/*-----------------------------------------------------------------------------
 * @file   tpcan.c
 * @brief  CAN BUS protocol for TurboPilot (DroneCAN)
 * @author WLH<wanglehui8357@163.com> Copyright (C) 2021~2099.
-----------------------------------------------------------------------------*/
#include "tpcan.h"
#include "hal_can.h"
#include "hal_irq.h"
#include "hal_hrt.h"
#include "dbg_tool.h"

/*-----------------------------------------------------------------------------
 * TRACE Tools
 *---------------------------------------------------------------------------*/
#define USE_TPCAN_TRACE 0u

#if (USE_TPCAN_TRACE == 0u)
#define TPCAN_TRACE(...)
#define TPCAN_TRACE_I(...)
#define TPCAN_TRACE_W(...)
#define TPCAN_TRACE_E(...)
#else
#define TPCAN_TRACE(...)   trace_out(__VA_ARGS__)
#define TPCAN_TRACE_I(...) trace_info(__VA_ARGS__)
#define TPCAN_TRACE_W(...) trace_warn(__VA_ARGS__)
#define TPCAN_TRACE_E(...) trace_error(__VA_ARGS__)
#endif

/*-----------------------------------------------------------------------------
 * macro define
-----------------------------------------------------------------------------*/
#define TRANSFER_TIMEOUT_USEC         1000000UL
#define MAX_TRANS_ID                  (32u)
#define SRC_ID_FROM_ID(x)             ((U8)(((x) >> 0) & 0x7Fu))
#define DST_ID_FROM_ID(x)             ((U8)(((x) >> 8) & 0x7Fu))
#define SRV_NOT_MSG_FROM_ID(x)        (0u != (((x) >> 7) & 0x01u))
#define REQ_NOT_RSP_FROM_ID(x)        (0u != (((x) >> 15) & 0x01u))
#define PRIORITY_FROM_ID(x)           ((U8)(((x) >> 24) & 0x1Fu))
#define MSG_TYPE_FROM_ID(x)           ((U16)(((x) >> 8) & 0xFFFFu))
#define SRV_TYPE_FROM_ID(x)           ((U8)(((x) >> 16) & 0xFFu))
#define TRANSFER_ID_FROM_TAIL_BYTE(x) ((U8)((x) & 0x1Fu))

#define MAKE_TRANS_DESC(dt_id, trans_type, src_id, dst_id) (((U32)(dt_id)) | (((U32)(trans_type)) << 16) | (((U32)(src_id)) << 18) | (((U32)(dst_id)) << 25))

/* utils for tail byte info check */
#define IS_START_OF_TRANSFER(x) (0u != (((x) >> 7) & 0x01u))
#define IS_END_OF_TRANSFER(x)   (0u != (((x) >> 6) & 0x01u))
#define TOGGLE_BIT(x)           ((U8)(((x) >> 5) & 0x01u))
#define TPCAN_CAN_EXT_ID_MASK   0x1FFFFFFFu
#define TPCAN_CAN_STD_ID_MASK   0x000007FFu
#define TPCAN_CAN_FRAME_EFF     (1UL << 31) /* Extended frame format     */
#define TPCAN_CAN_FRAME_RTR     (1UL << 30) /* Remote transmission frame */
#define TPCAN_BROADCAST_NODE_ID 0u          /* Node ID for Broadcast     */

/*-----------------------------------------------------------------------------
 * local variable
-----------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------
 * local functions
-----------------------------------------------------------------------------*/
static void increment_transfer_id(U8* trans_id)
{
    assert(trans_id != NULL);

    if (trans_id != NULL)
    {
        *trans_id += 1u;
        if (*trans_id >= MAX_TRANS_ID)
        {
            *trans_id = 0u;
        }
    }
}

static U16 crc_add_byte(U16 crc_val, const U8 byte)
{
    crc_val ^= (U16)((U16)byte << 8);
    for (int j = 0; j < 8; j++)
    {
        if (0u != (crc_val & 0x8000u))
        {
            crc_val = (U16)((U16)(crc_val << 1) ^ 0x1021u);
        }
        else
        {
            crc_val = (U16)(crc_val << 1);
        }
    }
    return crc_val;
}

static U16 crc_add_signature(U16 crc_val, U64 signature)
{
    for (U16 shift_val = 0u; shift_val < 64u; shift_val = (U16)(shift_val + 8u))
    {
        crc_val = crc_add_byte(crc_val, (U8)(signature >> shift_val));
    }
    return crc_val;
}

static U16 crc_add(U16 crc_val, const U8* bytes, size_t len)
{
    while ((len--) != 0u)
    {
        crc_val = crc_add_byte(crc_val, *bytes++);
    }
    return crc_val;
}
/**
 * @brief create new buffer block for RxTransfer
 */
static tpcan_buf_blk* create_buffer_block(tpcan_inst_t* ins)
{
    U8 i;

    for (i = 0u; i < TPCAN_BUF_BLOCK_MAX_CNT; i++)
    {
        /* find unused item and mark used flag */
        if (0u == ins->blk_items[i].used)
        {
            CRITICAL_SECTION_ENTER
            ins->blk_items[i].used = 1;
            ins->blk_items[i].data[0] = 0;
            CRITICAL_SECTION_LEAVE
            break;
        }
    }

    return i == TPCAN_BUF_BLOCK_MAX_CNT ? (tpcan_buf_blk*)NULL : &ins->blk_items[i];
}
/**
 * @brief returns transfer type from id
*/
static tpcan_trans_type extract_transfer_type(const U32 id)
{
    if (!SRV_NOT_MSG_FROM_ID(id))
    {
        return TRANS_TYPE_BCST;
    }

    if (REQ_NOT_RSP_FROM_ID(id) == 1u)
    {
        return TRANS_TYPE_REQ;
    }

    return TRANS_TYPE_RSP;
}
/**
 * @brief returns data type from frame ID
*/
static U16 extract_data_type(const U32 id)
{
    if (extract_transfer_type(id) == TRANS_TYPE_BCST)
    {
        U16 dt_id = MSG_TYPE_FROM_ID(id);
        if (SRC_ID_FROM_ID(id) == TPCAN_BROADCAST_NODE_ID)
        {
            /* anonymous transfer Not supported */
        }
        return dt_id;
    }
    return (U16)SRV_TYPE_FROM_ID(id);
}
/**
 * @brief returns pointer to the rx queue item of transfer descriptor or null if not found
 */
static tpcan_rxqueue_item_t* find_rxqueue_item(tpcan_inst_t* ins, const U32 trans_desc)
{
    unsigned count = 0u;
    list_item_t* node = list_first_item(&ins->rx_queue);
    while (node != NULL)
    {
        tpcan_rxqueue_item_t* ret = (tpcan_rxqueue_item_t*)node;
        if (ret->descriptor == trans_desc)
        {
            return ret;
        }

        /* Move to next item in list */
        node = list_next_item(node);
        if (count++ > TPCAN_MAX_RX_QUEUE_CAPACITY)
        {
            TPCAN_TRACE_E("[TPCAN]:List error");
            break;
        }
    }

    return (tpcan_rxqueue_item_t*)NULL;
}
/**
 * @brief Create rxqueue item from allocator
 */
static tpcan_rxqueue_item_t* create_rxqueue_item(tpcan_inst_t* ins, const U32 trans_desc)
{
    for (unsigned i = 0u; i < TPCAN_MAX_RX_QUEUE_CAPACITY; i++)
    {
        tpcan_rxqueue_item_t* rx_item = &ins->rx_items[i];
        if (0u == rx_item->used_flag)
        {
            CRITICAL_SECTION_ENTER
            rx_item->used_flag = 1u;
            rx_item->descriptor = trans_desc;
            rx_item->blk = (tpcan_buf_blk*)NULL;
            list_item_init(&rx_item->lt);
            CRITICAL_SECTION_LEAVE

            (void)list_add_to_start(&ins->rx_queue, &rx_item->lt);
            return rx_item;
        }
    }

    return (tpcan_rxqueue_item_t*)NULL;
}
/**
 * @brief Traverses the list of tpcan_rxqueue_item_t's
 * and returns a pointer to the tpcan_rxqueue_item_t
 * with either the Id or a new one at the end
 */
static tpcan_rxqueue_item_t* traverse_rxqueue_item(tpcan_inst_t* ins, const U32 trans_desc)
{
    tpcan_rxqueue_item_t* item = find_rxqueue_item(ins, trans_desc);
    if (NULL == item)
    {
        item = create_rxqueue_item(ins, trans_desc);
    }

    return item;
}
/**
 * @brief Release item's payload buffer block in rx queue
 */
static void release_rxqueue_item_payload(tpcan_rxqueue_item_t* item)
{
    if (NULL != item->blk)
    {
        CRITICAL_SECTION_ENTER
        item->blk->used = 0;
        CRITICAL_SECTION_LEAVE
    }
}
/**
 * @brief Returns true if priority of rhs is higher than id
 */
static bool is_priority_higher(const U32 rhs, const U32 id)
{
    const U32 clean_id = id & TPCAN_CAN_EXT_ID_MASK;
    const U32 rhs_clean_id = rhs & TPCAN_CAN_EXT_ID_MASK;

    /* STD vs EXT -
     * if 11 most significant bits are the same, EXT loses. */
    const bool ext = (id & TPCAN_CAN_FRAME_EFF) != 0u;
    const bool rhs_ext = (rhs & TPCAN_CAN_FRAME_EFF) != 0u;
    if (ext != rhs_ext)
    {
        const U32 arb11 = ext ? (clean_id >> 18) : clean_id;
        const U32 rhs_arb11 = rhs_ext ? (rhs_clean_id >> 18) : rhs_clean_id;
        if (arb11 != rhs_arb11)
        {
            return arb11 < rhs_arb11;
        }
        return rhs_ext;
    }

    /* RTR vs Data frame -
     * if frame identifiers and frame types are the same, RTR loses. */
    const bool rtr = (id & TPCAN_CAN_FRAME_RTR) != 0u;
    const bool rhs_rtr = (rhs & TPCAN_CAN_FRAME_RTR) != 0u;
    if (clean_id == rhs_clean_id && rtr != rhs_rtr)
    {
        return rhs_rtr;
    }

    /* Plain ID arbitration - greater value loses. */
    return clean_id < rhs_clean_id;
}
/**
 * @brief put data byte to buffer block
 */
static bool push_byte(tpcan_buf_blk* block, const U8 data)
{
    if (block->data[0] < (TPCAN_BUF_BLOCK_DATA_SIZE - 1u))
    {
        block->data[0]++;
        block->data[block->data[0]] = data;
        return true;
    }

    return false;
}
/**
 * @brief pushes data into the buffer block
 * Fills the buffer head, then appends data to buffer blocks
*/
static tpcan_state buf_block_push_bytes(tpcan_inst_t* ins, tpcan_rxqueue_item_t* item, const U8* data, const U8 len)
{
    if (NULL == item->blk)
    {
        item->blk = create_buffer_block(ins);
        if (item->blk == NULL)
        {
            return TPCAN_ERR_OUT_OF_MEMORY;
        }
    }

    for (U8 i = 0; i < len; i++)
    {
        if (!push_byte(item->blk, data[i]))
        {
            return TPCAN_ERR_OUT_OF_MEMORY;
        }
    }

    item->payload_len += len;
    return TPCAN_ERR_NONE;
}
/**
 * @brief Creates new tx queue item from allocator
*/
static tpcan_txqueue_item_t* create_txqueue_item(tpcan_inst_t* ins)
{
    for (unsigned i = 0u; i < TPCAN_MAX_TX_QUEUE_CAPACITY; i++)
    {
        if (ins->tx_items[i].is_used)
        {
            continue;
        }
        /* find unused item and mark used flag */
        CRITICAL_SECTION_ENTER
        ins->tx_items[i].is_used = true;
        list_item_init(&ins->tx_items[i].lt);
        CRITICAL_SECTION_LEAVE
        return &ins->tx_items[i];
    }

    return (tpcan_txqueue_item_t*)NULL;
}
/**
 * @brief Puts frame on the TX queue. Higher priority placed first
*/
static void push_tx_queue(tpcan_inst_t* ins, tpcan_txqueue_item_t* item)
{
    list_item_t* node = list_first_item(&ins->tx_queue);
    if (NULL == node)
    {
        (void)list_add_to_start(&ins->tx_queue, &item->lt);
        return;
    }

    while (NULL != node)
    {
        tpcan_txqueue_item_t* tx_item = (tpcan_txqueue_item_t*)node;
        if (is_priority_higher(tx_item->frame.id, item->frame.id))
        {
            (void)list_add_before(&ins->tx_queue, node, &item->lt);
            return;
        }
        node = list_next_item(node);
    }

    /* item has the lowest priority */
    (void)list_add_to_end(&ins->tx_queue, &item->lt);
}
/**
 * @brief Puts payload to tx queue by change to tpcan_txqueue_item_t
*/
static tpcan_state enqueue_tx_frames(tpcan_inst_t* ins,
                                     const U32 can_id,
                                     const U8 trans_id,
                                     const U16 crc,
                                     const U8* payload,
                                     const U16 len)
{
    assert((can_id & TPCAN_CAN_EXT_ID_MASK) == can_id); // Flags must be cleared

    if (payload == NULL || len == 0u)
    {
        return TPCAN_ERR_INVALID_PARAM;
    }

    const unsigned frame_max_len = ins->fd_mode ? CAN_FD_MAX_DATA_LEN : CAN_STD_MAX_DATA_LEN;
    if (len < frame_max_len - 1u)
    {
        /* Single frame transfer */
        tpcan_txqueue_item_t* tx_item = create_txqueue_item(ins);
        if (NULL == tx_item)
        {
            TPCAN_TRACE_E("[TPCAN]:Out of memory");
            return TPCAN_ERR_OUT_OF_MEMORY;
        }

        (void)memcpy(tx_item->frame.data, payload, len);
        /* add generated tail byte */
        tx_item->frame.len = (U8)(len + 1u);
        tx_item->frame.data[len] = (U8)(0xC0u | (trans_id & 0x1Fu));
        tx_item->frame.id = can_id;
        push_tx_queue(ins, tx_item);
        return TPCAN_ERR_NONE;
    }

    /* Multi frames transfer */
    U8 data_index = 0u;
    U8 toggle = 0u;
    
    /* flag for start/end of transfer in tail byte bit5 */
    U8 sot_eot = 0u;
    while (len != data_index)
    {
        tpcan_txqueue_item_t* tx_item = create_txqueue_item(ins);
        if (NULL == tx_item)
        {
            /* TODO: Purge all frames enqueued so far */
            TPCAN_TRACE_E("[TPCAN]:Out of memory");
            return TPCAN_ERR_OUT_OF_MEMORY;
        }

        U8 i = 0u;
        if (data_index == 0u)
        {
            /* create header frame: add crc */
            tx_item->frame.data[i++] = WORD_LO(crc);
            tx_item->frame.data[i++] = WORD_HI(crc);
            sot_eot = 0x80u; // Start of transfer
        }

        for (; i < frame_max_len - 1u && data_index < len; i++, data_index++)
        {
            tx_item->frame.data[i] = payload[data_index];
        }

        // tail byte
        if (data_index == len)
        {
            sot_eot = 0x40u; // End of transfer
        }
        tx_item->frame.data[i] = (U8)(sot_eot | (toggle << 5) | (trans_id & 0x1Fu));
        tx_item->frame.id = can_id;
        tx_item->frame.len = i + 1u;
        push_tx_queue(ins, tx_item);

        toggle = toggle == 0u ? 1u : 0u;
        sot_eot = 0u; // Middle of transfer
    }

    return TPCAN_ERR_NONE;
}
/**
 * @brief prepare the rxqueue item for the next transfer.
 */
static void prepare_for_next_transfer(tpcan_rxqueue_item_t* rx)
{
    rx->used_flag = 0;
    rx->trans_id = 0;
    rx->payload_len = 0;
    rx->next_toggle = 0;
    list_item_init(&rx->lt);
}
/**
 * @brief This function use to release pool blocks that store the payload of the transfer.
 */
static void release_rx_transfer_payload(tpcan_inst_t* ins, tpcan_rx_trans_t* transfer)
{
    assert(NULL != transfer->rx_item);

    release_rxqueue_item_payload(transfer->rx_item);
    list_remove_item(&ins->rx_queue, &transfer->rx_item->lt);
    transfer->rx_item = NULL;
}
/**
 * @brief Returns a pointer to the top priority frame in the TX queue.
 * @return Get NULL if the TX queue is empty.
 */
static tpcan_frame_t* peek_tx_queue(const tpcan_inst_t* ins)
{
    if (NULL == ins->tx_queue.first_item)
    {
        return (tpcan_frame_t*)NULL;
    }

    tpcan_txqueue_item_t* item = (tpcan_txqueue_item_t*)ins->tx_queue.first_item;
    return &(item->frame);
}
/**
 * @brief Removes the top priority frame from the TX queue.
 * The application will call this function after peek_tx_queue
 * once the obtained frame has been processed.
 */
static void pop_tx_queue(tpcan_inst_t* ins)
{
    tpcan_txqueue_item_t* item = (tpcan_txqueue_item_t*)list_remove_first_item(&ins->tx_queue);
    if (NULL != item)
    {
        CRITICAL_SECTION_ENTER
        item->is_used = false;
        CRITICAL_SECTION_LEAVE
    }
}

/*-----------------------------------------------------------------------------
 * public functions
-----------------------------------------------------------------------------*/
void tpcan_init(tpcan_inst_t* ins, const U8 node_id, const bool fd_mode)
{
    assert(ins != NULL);
    if (NULL == ins)
    {
        return;
    }

    (void)memset(ins, 0, sizeof(tpcan_inst_t));
    list_init(&ins->tx_queue, TPCAN_MAX_TX_QUEUE_CAPACITY);
    list_init(&ins->rx_queue, TPCAN_MAX_RX_QUEUE_CAPACITY);
    ins->node_id = node_id;
    ins->fd_mode = fd_mode;
}

tpcan_state tpcan_broadcast(tpcan_inst_t* ins, tpcan_msg_t* msg)
{
    assert(NULL != ins);
    assert(NULL != msg);
    if (NULL == ins || NULL == msg)
    {
        return TPCAN_ERR_INVALID_PARAM;
    }
    assert(NULL != msg->trans_id);
    if (NULL == msg->trans_id)
    {
        return TPCAN_ERR_INVALID_PARAM;
    }

    if (msg->payload == NULL && msg->len > 0u)
    {
        return TPCAN_ERR_INVALID_PARAM;
    }

    if (msg->priority > (U8)TPCAN_TRANS_PRIO_LOWEST)
    {
        return TPCAN_ERR_INVALID_PARAM;
    }

    if (ins->node_id == TPCAN_BROADCAST_NODE_ID)
    {
        /* anonymous transfer Not supported */
        return TPCAN_ERR_NODE_ID_NOT_SET;
    }

    const unsigned frame_max_len = ins->fd_mode ? CAN_FD_MAX_DATA_LEN : CAN_STD_MAX_DATA_LEN;
    U16 crc = 0xFFFFU;
    if (msg->len > (frame_max_len - 1u))
    {
        crc = crc_add_signature(crc, msg->signature);
        crc = crc_add(crc, msg->payload, msg->len);
    }

    // Ref message frame ID format
    const U32 can_id = (U32)msg->priority << 24 |
                       (U32)msg->dt_id << 8 |
                       (U32)ins->node_id;
    const tpcan_state state = enqueue_tx_frames(ins, can_id, *msg->trans_id, crc, msg->payload, msg->len);
    if (state == TPCAN_ERR_NONE)
    {
        increment_transfer_id(msg->trans_id);
    }
    else
    {
        TPCAN_TRACE_E("[TPCAN]:Failed to enqueue tx frames, error: %d", state);
    }

    return state;
}

tpcan_state tpcan_send_req(tpcan_inst_t* ins, const U8 dst_node_id, const tpcan_msg_t* msg)
{
    assert(NULL != ins);
    assert(NULL != msg);
    if (NULL == ins || NULL == msg)
    {
        return TPCAN_ERR_INVALID_PARAM;
    }

    assert(NULL != msg->trans_id);
    if (NULL == msg->trans_id)
    {
        return TPCAN_ERR_INVALID_PARAM;
    }

    tpcan_state state = TPCAN_ERR_NONE;

    if (msg->payload == NULL && msg->len > 0u || msg->len > 127u)
    {
        state = TPCAN_ERR_INVALID_PARAM;
    }

    if (msg->priority > (U8)TPCAN_TRANS_PRIO_LOWEST)
    {
        state = TPCAN_ERR_INVALID_PARAM;
    }

    if (ins->node_id == 0u)
    {
        state = TPCAN_ERR_NODE_ID_NOT_SET;
    }

    if (TPCAN_ERR_NONE == state)
    {
        // Ref service frame ID format
        const U32 can_id = (U32)msg->priority << 24 |
                           (U32)msg->dt_id << 16 |
                           (U32)TRANS_TYPE_REQ << 15 |
                           (U32)dst_node_id << 8 |
                           (U32)(1u << 7) |
                           (U32)ins->node_id;

        U16 crc = 0xFFFFU;
        const unsigned frame_max_len = ins->fd_mode ? CAN_FD_MAX_DATA_LEN : CAN_STD_MAX_DATA_LEN;
        if (msg->len > (frame_max_len - 1u))
        {
            /* calculate crc bytes for multi-frame header */
            crc = crc_add_signature(crc, msg->signature);
            crc = crc_add(crc, msg->payload, msg->len);
        }

        state = enqueue_tx_frames(ins, can_id, *msg->trans_id, crc, msg->payload, msg->len);
        increment_transfer_id(msg->trans_id);
    }

    return state;
}

tpcan_state tpcan_send_rsp(tpcan_inst_t* ins, const U8 dst_node_id, const tpcan_msg_t* msg)
{
    assert(NULL != ins);
    assert(NULL != msg);

    if (NULL == ins || NULL == msg)
    {
        return TPCAN_ERR_INVALID_PARAM;
    }

    assert(NULL != msg->trans_id);
    if (NULL == msg->trans_id)
    {
        return TPCAN_ERR_INVALID_PARAM;
    }

    tpcan_state state = TPCAN_ERR_NONE;
    if (msg->payload == NULL && msg->len > 0u || msg->len > 127u)
    {
        state = TPCAN_ERR_INVALID_PARAM;
    }

    if (msg->priority > (U8)TPCAN_TRANS_PRIO_LOWEST)
    {
        state = TPCAN_ERR_INVALID_PARAM;
    }

    if (ins->node_id == 0u)
    {
        state = TPCAN_ERR_NODE_ID_NOT_SET;
    }

    const U32 can_id = (U32)msg->priority << 24 |
                       (U32)msg->dt_id << 16 |
                       (U32)TRANS_TYPE_RSP << 15 |
                       (U32)dst_node_id << 8 |
                       (U32)(1u << 7) |
                       (U32)ins->node_id;

    U16 crc = 0xFFFFU;
    const unsigned frame_max_len = ins->fd_mode ? CAN_FD_MAX_DATA_LEN : CAN_STD_MAX_DATA_LEN;
    if (msg->len > (frame_max_len - 1u))
    {
        /* calculate crc bytes for multi-frame header */
        crc = crc_add_signature(crc, msg->signature);
        crc = crc_add(crc, msg->payload, msg->len);
    }

    if (TPCAN_ERR_NONE == state)
    {
        state = enqueue_tx_frames(ins, can_id, *msg->trans_id, crc, msg->payload, msg->len);
    }

    return state;
}

void tpcan_flush_tx_queue(tpcan_inst_t* ins, const U8 port)
{
    assert(NULL != ins);
    if (NULL == ins)
    {
        return;
    }

    tpcan_frame_t* fm;
    do
    {
        fm = peek_tx_queue(ins);
        if (NULL != fm)
        {
            const bool ret = hal_can_transmit(port, fm->id, fm->data, fm->len);
            if (!ret)
            {
                TPCAN_TRACE_E("[TPCAN]:Transmit frame failed");
            }
            pop_tx_queue(ins);
        }
    } while (NULL != fm);
}

void tpcan_handle_can_frame(tpcan_inst_t* ins, can_frame_t* frame)
{
    assert(NULL != ins);
    assert(NULL != frame);
    if (NULL == ins || NULL == frame)
    {
        return;
    }

    const tpcan_trans_type trans_type = extract_transfer_type(frame->id);
    const U8 dst_node_id = trans_type == TRANS_TYPE_BCST ? (U8)TPCAN_BROADCAST_NODE_ID : DST_ID_FROM_ID(frame->id);

    /* Unsupported frame size */
    const U8 frame_max_len = ins->fd_mode ? CAN_FD_MAX_DATA_LEN : CAN_STD_MAX_DATA_LEN;

    /* Check Rx frame real data length (for CAN-FD DLC)*/
    while (frame->len > CAN_STD_MAX_DATA_LEN)
    {
        if (0u != frame->data[frame->len - 1u])
        {
            break;
        }
        frame->len--; /* remove emptry byte '\0' */
    }

    if (frame->len < 1u || frame->len > frame_max_len)
    {
        return;
    }

    if (trans_type != TRANS_TYPE_BCST && dst_node_id != ins->node_id)
    {
        return; /* CAN node address mismatch */
    }

    const U8 priority = PRIORITY_FROM_ID(frame->id);
    const U8 src_node_id = SRC_ID_FROM_ID(frame->id);
    const U16 dt_id = extract_data_type(frame->id);
    const U32 trans_desc = MAKE_TRANS_DESC(dt_id, trans_type, src_node_id, dst_node_id);

    const U8 tail_byte = frame->data[frame->len - 1u];
    const bool is_start_frame = IS_START_OF_TRANSFER(tail_byte);
    const bool is_end_frame = IS_END_OF_TRANSFER(tail_byte);
    const U8 trans_id = TRANSFER_ID_FROM_TAIL_BYTE(tail_byte);

    tpcan_rxqueue_item_t* rx_item;
    U64 signature = 0u;
    if (is_start_frame) // Start of transfer frame
    {
        bool is_accept = true;
        if(NULL != ins->should_accept)
        {
            is_accept = ins->should_accept(ins, &signature, dt_id, trans_type, src_node_id);
        }

        if (is_accept)
        {
            rx_item = traverse_rxqueue_item(ins, trans_desc);
            if (rx_item == NULL)
            {
                TPCAN_TRACE_E("[TPCAN]:No space for frame");
                return;
            }
            rx_item->next_toggle = 0;
            rx_item->trans_id = trans_id;
            rx_item->payload_len = 0;
        }
        else
        {
            return; /* The application doesn't want this transfer */
        }
    }
    else // Middle or end of transfer frame
    {
        rx_item = find_rxqueue_item(ins, trans_desc);
        if (rx_item == NULL)
        {
            // TPCAN_TRACE_E("[TPCAN]:Unexpected rx item");
            // Too nosy log, comment it out (for NON-TPCAN protocol frames)
            return;
        }
    }

    /* All paths that lead to NULL should be terminated with return above */
    assert(rx_item != NULL);

    if (TOGGLE_BIT(tail_byte) != rx_item->next_toggle)
    {
        TPCAN_TRACE_E("[TPCAN]:Wrong toggle");
        return;
    }

    if (trans_id != rx_item->trans_id)
    {
        TPCAN_TRACE_E("[TPCAN]:Unexpected trans id (%u!=%u)", trans_id, rx_item->trans_id);
        return;
    }

    const U64 ts_us = hal_hrt_get_us();
    tpcan_state state;

    /* single frame transfer */
    if (is_start_frame && is_end_frame)
    {
        state = buf_block_push_bytes(ins, rx_item, frame->data, (U8)(frame->len - 1u));
        if (TPCAN_ERR_NONE != state)
        {
            release_rxqueue_item_payload(rx_item);
            prepare_for_next_transfer(rx_item);
            return;
        }
        rx_item->ts_us = ts_us;
        tpcan_rx_trans_t rx_transfer = {
            .ts_us = ts_us,
            .rx_item = rx_item,
            .dt_id = dt_id,
            .trans_type = (U8)trans_type,
            .trans_id = trans_id,
            .priority = priority,
            .src_node_id = src_node_id
        };

        if (NULL != ins->on_reception)
        {
            ins->on_reception(ins, &rx_transfer);
        }

        release_rx_transfer_payload(ins, &rx_transfer);
        prepare_for_next_transfer(rx_item);
        return;
    }

    if (is_start_frame && !is_end_frame)
    {
        /* Beginning of multi frame transfer */
        if (frame->len <= 3u)
        {
            TPCAN_TRACE_E("[TPCAN]:Not enough data");
            return;
        }
        rx_item->ts_us = ts_us;

        /* remove the crc bytes (+tail byte) */
        const U8* data_ptr = &frame->data[2];
        const U8 data_len = (U8)(frame->len - 3U);
        state = buf_block_push_bytes(ins, rx_item, data_ptr, data_len);
        if (TPCAN_ERR_NONE != state)
        {
            release_rxqueue_item_payload(rx_item);
            prepare_for_next_transfer(rx_item);
            return;
        }

        rx_item->payload_crc = GEN_WORD(frame->data[0], frame->data[1]);
        rx_item->crc_val = crc_add_signature(0xFFFFU, signature);
        rx_item->crc_val = crc_add((U16)rx_item->crc_val, data_ptr, data_len);
        rx_item->trans_id = trans_id;
    }
    else if (!is_start_frame && !is_end_frame)
    {
        /* Middle of a multi-frame transfer */
        const U8 data_len = (U8)(frame->len - 1u);
        state = buf_block_push_bytes(ins, rx_item, frame->data, data_len);
        if (TPCAN_ERR_NONE != state)
        {
            release_rxqueue_item_payload(rx_item);
            prepare_for_next_transfer(rx_item);
            return;
        }
        rx_item->crc_val = crc_add((U16)rx_item->crc_val, frame->data, data_len);
    }
    else
    {
        /* End of a multi-frame transfer */
        const U8 data_len = (U8)(frame->len - 1u);
        state = buf_block_push_bytes(ins, rx_item, frame->data, data_len);
        if (TPCAN_ERR_NONE != state)
        {
            release_rxqueue_item_payload(rx_item);
            prepare_for_next_transfer(rx_item);
            return;
        }

        tpcan_rx_trans_t rx_transfer = {
            .ts_us = ts_us,
            .rx_item = rx_item,
            .dt_id = dt_id,
            .trans_type = (U8)trans_type,
            .trans_id = trans_id,
            .priority = priority,
            .src_node_id = src_node_id
        };

        /* CRC validation */
        rx_item->crc_val = crc_add((U16)rx_item->crc_val, frame->data, data_len);
        if (rx_item->crc_val == rx_item->payload_crc)
        {
            if (NULL != ins->on_reception)
            {
                ins->on_reception(ins, &rx_transfer);
            }
        }
        else
        {
            TPCAN_TRACE_E("[TPCAN]:ID=%d CRC error %04X != %04X", dt_id, rx_item->crc_val, rx_item->payload_crc);
        }

        /* release pool blocks that are used to store the payload of the transfer */
        release_rx_transfer_payload(ins, &rx_transfer);
        prepare_for_next_transfer(rx_item);
        return;
    }

    /* toggle bit for next frame */
    rx_item->next_toggle = rx_item->next_toggle == 0u ? 1u : 0u;
}

void tpcan_cleanup_stale_transfers(tpcan_inst_t* ins, const U64 ts_us)
{
    unsigned count = 0;
    list_item_t* node = list_first_item(&ins->rx_queue);
    while (node != NULL)
    {
        list_item_t* next = list_next_item(node);

        tpcan_rxqueue_item_t* rx_item = (tpcan_rxqueue_item_t*)node;
        if (ts_us - rx_item->ts_us > TRANSFER_TIMEOUT_USEC)
        {
            CRITICAL_SECTION_ENTER
            release_rxqueue_item_payload(rx_item);
            list_remove_item(&ins->rx_queue, &rx_item->lt);
            rx_item->used_flag = 0;
            CRITICAL_SECTION_LEAVE
        }
        node = next;
        if (count++ > TPCAN_MAX_RX_QUEUE_CAPACITY)
        {
            TPCAN_TRACE_E("[TPCAN]:List error cleanup");
            break;
        }
    }
}
